fix(behavior_velocity_planner): fix velocity interpolation changing the stop position inappropriately #623

TakaHoribe · 2022-04-02T06:47:14Z

Description

Interpolation in the behavior_velocity_planner sometimes calculates an inappropriate velocity.

This PR fixes the issue and adds associated unit tests.

Current implementation:

OK case: s_in = [0, 2.1], v_in = [5, 0] -> s_out = [0, 1, 2, 2.1, 3], v_out = [5, 5, 5, 0, 0].
- stop point is s=2.1 which is OK.
BAD case: s_in = [0, 2.001], v_in = [5, 0] -> s_out = [0, 1, 2, 3], v_out = [5, 5, 5, 0].
- stop point is s=3 which is BAD because the original stop point is s=2.001.

The cause is that the original path point is overwritten by a new constant-interval point when they are at a close distance. In this PR, the original path points are never overwritten and the new constant-interval point is removed when they are close.

With this PR:

Original OK case: s_in = [0, 2.1], v_in = [5, 0] -> s_out = [0, 1, 2, 2.1, 3], v = [5, 5, 5, 0, 0].
- stop point is s=2.1 which is OK. (Not changed anything)
Original BAD case: s_in = [0, 2.001], v_in = [5, 0] -> s_out = [0, 1, 2.001, 3], v = [5, 5, 5, 0].
- stop point is s=2.001 which is OK.

Tests performed

I've tested with the rosbag which causes this bug.
Also with the unit test.
Please check that the unit test fails in the original implementation and succeeds with this PR.

Notes for reviewers

Pre-review checklist for the PR author

The PR author must check the checkboxes below when creating the PR.

I've confirmed the contribution guidelines.
The PR follows the pull request guidelines.

In-review checklist for the PR reviewers

The PR reviewers must check the checkboxes below before approval.

The PR follows the pull request guidelines.
The PR has been properly tested.
The PR has been reviewed by the code owners.

Post-review checklist for the PR author

The PR author must check the checkboxes below before merging.

There are no open discussions or they are tracked via tickets.
The PR is ready for merge.

After all checkboxes are checked, anyone who has write access can merge the PR.

…he stop position inappropriately Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

codecov · 2022-04-02T07:12:22Z

Codecov Report

Merging #623 (ac73fdc) into main (5153502) will decrease coverage by 4.20%.
The diff coverage is 46.00%.

@@            Coverage Diff            @@
##             main    #623      +/-   ##
=========================================
- Coverage   10.74%   6.53%   -4.21%     
=========================================
  Files         713      68     -645     
  Lines       50671    5097   -45574     
  Branches     6572     569    -6003     
=========================================
- Hits         5443     333    -5110     
+ Misses      40821    4548   -36273     
+ Partials     4407     216    -4191

Impacted Files	Coverage Δ
...ior_velocity_planner/test/src/test_utilization.cpp	`53.62% <45.00%> (-11.90%)`	⬇️
...ocity_planner/src/utilization/path_utilization.cpp	`16.80% <50.00%> (+16.80%)`	⬆️
...troller/src/joy_controller/joy_controller_node.cpp
...ion/include/shape_estimation/filter/car_filter.hpp
...elevation_map_loader/elevation_map_loader_node.hpp
...erception/shape_estimation/lib/corrector/utils.cpp
...association/solver/mu_successive_shortest_path.hpp
...ose_button_panel/src/initial_pose_button_panel.cpp
...on/include/shape_estimation/model/bounding_box.hpp
...ctory_follower/include/trajectory_follower/mpc.hpp
... and 638 more

Continue to review full report at Codecov.

Legend - Click here to learn more
Δ = absolute <relative> (impact), ø = not affected, ? = missing data
Powered by Codecov. Last update 5153502...ac73fdc. Read the comment docs.

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

planning/behavior_velocity_planner/src/utilization/path_utilization.cpp

taikitanaka3 · 2022-04-06T04:44:04Z

planning/behavior_velocity_planner/src/utilization/path_utilization.cpp

-  for (const double s : s_tmp) {
-    if (!s_out.empty() && std::fabs(s_out.back() - s) < epsilon) {
-      continue;
+    if (!has_almost_same_value(s_out, s)) {


planning/behavior_velocity_planner/src/utilization/path_utilization.cpp

taikitanaka3

LGTM

tested by real vehicle

…he stop position inappropriately (#623) * fix(behavior_velocity_planner): fix velocity interpolation changing the stop position inappropriately Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com> * fix typo Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com> * add test for interpolation Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com> * fix build warning Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

…anging the stop position inappropriately (#623)" This reverts commit 90ee0ae.

…he stop position inappropriately (autowarefoundation#623) * fix(behavior_velocity_planner): fix velocity interpolation changing the stop position inappropriately Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com> * fix typo Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com> * add test for interpolation Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com> * fix build warning Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

…utowarefoundation#3991) (autowarefoundation#623) * feat(behavior_path_planner): enable delay lane change when necessary * update * update * feat(behavior_path_planner): add a function to judge a parked vehicle * update * update * update * update * update * update * update * update * Update planning/behavior_path_planner/src/utils/lane_change/utils.cpp * update name --------- Signed-off-by: yutaka <purewater0901@gmail.com> Co-authored-by: Yutaka Shimizu <43805014+purewater0901@users.noreply.github.com>

* feat: change runtime type * fix: typo * Update .webauto-ci.yml --------- Co-authored-by: Makoto Tokunaga <vios-fish@users.noreply.github.com>

…n#623) Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp>

fix(behavior_velocity_planner): fix velocity interpolation changing t…

9458314

…he stop position inappropriately Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

TakaHoribe requested a review from tkimura4 April 2, 2022 06:47

fix typo

a9a1e89

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

TakaHoribe added 2 commits April 2, 2022 17:05

add test for interpolation

05eb9f0

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

fix build warning

ac73fdc

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

TakaHoribe requested a review from taikitanaka3 April 5, 2022 03:24

taikitanaka3 reviewed Apr 6, 2022

View reviewed changes

planning/behavior_velocity_planner/src/utilization/path_utilization.cpp Show resolved Hide resolved

taikitanaka3 reviewed Apr 6, 2022

View reviewed changes

planning/behavior_velocity_planner/src/utilization/path_utilization.cpp Show resolved Hide resolved

taikitanaka3 approved these changes Apr 6, 2022

View reviewed changes

TakaHoribe merged commit 90ee0ae into autowarefoundation:main Apr 6, 2022

TakaHoribe deleted the fix/behavior-velocity-interpolation-bug branch April 6, 2022 07:44

mehmetdogru mentioned this pull request Apr 6, 2022

Evaluate current planning pipeline #566

Closed

7 tasks

tier4-autoware-public-bot bot mentioned this pull request Apr 7, 2022

chore: sync upstream tier4/autoware.universe#33

Merged

YoheiMishina referenced this pull request in tier4/autoware.universe Apr 22, 2022

Revert "fix(behavior_velocity_planner): fix velocity interpolation ch…

12e0667

…anging the stop position inappropriately (#623)" This reverts commit 90ee0ae.

kyoichi-sugahara pushed a commit that referenced this pull request Sep 16, 2023

feat: change runtime type (#623)

767ab38

* feat: change runtime type * fix: typo * Update .webauto-ci.yml --------- Co-authored-by: Makoto Tokunaga <vios-fish@users.noreply.github.com>

iwatake2222 pushed a commit to iwatake2222/autoware.universe that referenced this pull request Jan 17, 2025

feat(intersection): yield initially on green light (autowarefoundatio…

2c08bc6

…n#623) Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp>

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

fix(behavior_velocity_planner): fix velocity interpolation changing the stop position inappropriately #623

fix(behavior_velocity_planner): fix velocity interpolation changing the stop position inappropriately #623

TakaHoribe commented Apr 2, 2022 •

edited

Loading

codecov bot commented Apr 2, 2022 •

edited

Loading

taikitanaka3 Apr 6, 2022

taikitanaka3 left a comment •

edited

Loading

fix(behavior_velocity_planner): fix velocity interpolation changing the stop position inappropriately #623

fix(behavior_velocity_planner): fix velocity interpolation changing the stop position inappropriately #623

Conversation

TakaHoribe commented Apr 2, 2022 • edited Loading

Description

Related links

Tests performed

Notes for reviewers

Pre-review checklist for the PR author

In-review checklist for the PR reviewers

Post-review checklist for the PR author

codecov bot commented Apr 2, 2022 • edited Loading

Codecov Report

taikitanaka3 Apr 6, 2022

Choose a reason for hiding this comment

taikitanaka3 left a comment • edited Loading

Choose a reason for hiding this comment

TakaHoribe commented Apr 2, 2022 •

edited

Loading

codecov bot commented Apr 2, 2022 •

edited

Loading

taikitanaka3 left a comment •

edited

Loading